
#include "matrix.h"


// /* 打印矩阵数据(在单片机中注释掉)*/
// void MatrixPrint(float* A , int num)
// {
//     for(int i = 1;i<= num;i++)
//     {
//       cout<< A[i-1] << endl;
//     }
// }

/**
 * @brief 求齐次变换矩阵的逆矩阵(待验证)
 * @note  成功输出1，失败输出0;
 * @param Titi ：待求逆的齐次变换矩阵
 * @param Titf : 齐次变换矩阵的逆矩阵
 * @param numI : 待求逆的齐次变换矩阵的元素个数
 * @param numF ：齐次变换矩阵的逆矩阵的元素个数
 */
int invtran(float *Titi, float *Titf, int numI ,int numF)
{
    if((numI != 16)||(numF != 16)) return 0;
  // finding the inverse of the homogeneous transformation matrix
  // first row
  Titf[0 * 4 + 0] = Titi[0 * 4 + 0];
  Titf[0 * 4 + 1] = Titi[1 * 4 + 0];
  Titf[0 * 4 + 2] = Titi[2 * 4 + 0];
  Titf[0 * 4 + 3] = -Titi[0 * 4 + 0] * Titi[0 * 4 + 3] - Titi[1 * 4 + 0] * Titi[1 * 4 + 3] - Titi[2 * 4 + 0] * Titi[2 * 4 + 3];
  // second row
  Titf[1 * 4 + 0] = Titi[0 * 4 + 1];
  Titf[1 * 4 + 1] = Titi[1 * 4 + 1];
  Titf[1 * 4 + 2] = Titi[2 * 4 + 1];
  Titf[1 * 4 + 3] = -Titi[0 * 4 + 1] * Titi[0 * 4 + 3] - Titi[1 * 4 + 1] * Titi[1 * 4 + 3] - Titi[2 * 4 + 1] * Titi[2 * 4 + 3];
  // third row
  Titf[2 * 4 + 0] = Titi[0 * 4 + 2];
  Titf[2 * 4 + 1] = Titi[1 * 4 + 2];
  Titf[2 * 4 + 2] = Titi[2 * 4 + 2];
  Titf[2 * 4 + 3] = -Titi[0 * 4 + 2] * Titi[0 * 4 + 3] - Titi[1 * 4 + 2] * Titi[1 * 4 + 3] - Titi[2 * 4 + 2] * Titi[2 * 4 + 3];
  // forth row
  Titf[3 * 4 + 0] = 0.0;
  Titf[3 * 4 + 1] = 0.0;
  Titf[3 * 4 + 2] = 0.0;
  Titf[3 * 4 + 3] = 1.0;
  return 1;
}

/**
 * @brief 将齐次变换矩阵变换到姿态(弧度制)(待验证)
 * @note  得到的欧拉角为ZYZ顺次欧拉角，成功输出1，失败输出0;
 * @param Tpt ：齐次变换矩阵
 * @param Xpt : 前三个元素为坐标，后三个元素为三轴欧拉角
 * @param numT : 齐次变换矩阵的元素个数
 * @param numX ：姿态数组的元素个数
 */
int tran2pos(float *Ttp, float *Xtp,int numX , int numT)
{
  if((numX != 6)||(numT != 16)) return 0;
  // pos from homogeneous transformation matrix
  Xtp[0] = Ttp[0 * 4 + 3];
  Xtp[1] = Ttp[1 * 4 + 3];
  Xtp[2] = Ttp[2 * 4 + 3];
  Xtp[4] = atan2(sqrt(Ttp[2 * 4 + 0] * Ttp[2 * 4 + 0] + Ttp[2 * 4 + 1] * Ttp[2 * 4 + 1]), Ttp[2 * 4 + 2]);
  Xtp[3] = atan2(Ttp[1 * 4 + 2] / sin(Xtp[4]), Ttp[0 * 4 + 2] / sin(Xtp[4]));
  Xtp[5] = atan2(Ttp[2 * 4 + 1] / sin(Xtp[4]), -Ttp[2 * 4 + 0] / sin(Xtp[4]));
  return 1;
}

/**
 * @brief 将姿态(弧度制)变换到齐次变换矩阵(待验证)
 * @note  输入的为ZYZ顺次欧拉角，成功输出1，失败输出0;
 * @param Xpt : 前三个元素为坐标，后三个元素为三轴欧拉角
 * @param Tpt ：齐次变换矩阵
 * @param numX ：姿态数组的元素个数
 * @param numT : 齐次变换矩阵的元素个数
 */
int pos2tran(float *Xpt, float *Tpt, int numX , int numT)
{
  //判断元素个数是否合法
  if((numX != 6)||(numT != 16)) return 0;
  // pos to homogeneous transformation matrix
  // first row
  Tpt[0 * 4 + 0] = cos(Xpt[3]) * cos(Xpt[4]) * cos(Xpt[5]) - sin(Xpt[3]) * sin(Xpt[5]);
  Tpt[0 * 4 + 1] = -cos(Xpt[3]) * cos(Xpt[4]) * sin(Xpt[5]) - sin(Xpt[3]) * cos(Xpt[5]);
  Tpt[0 * 4 + 2] = cos(Xpt[3]) * sin(Xpt[4]);
  Tpt[0 * 4 + 3] = Xpt[0];
  // second row
  Tpt[1 * 4 + 0] = sin(Xpt[3]) * cos(Xpt[4]) * cos(Xpt[5]) + cos(Xpt[3]) * sin(Xpt[5]);
  Tpt[1 * 4 + 1] = -sin(Xpt[3]) * cos(Xpt[4]) * sin(Xpt[5]) + cos(Xpt[3]) * cos(Xpt[5]);
  Tpt[1 * 4 + 2] = sin(Xpt[3]) * sin(Xpt[4]);
  Tpt[1 * 4 + 3] = Xpt[1];
  // third row
  Tpt[2 * 4 + 0] = -sin(Xpt[4]) * cos(Xpt[5]);
  Tpt[2 * 4 + 1] = sin(Xpt[4]) * sin(Xpt[5]);
  Tpt[2 * 4 + 2] = cos(Xpt[4]);
  Tpt[2 * 4 + 3] = Xpt[2];
  // forth row
  Tpt[3 * 4 + 0] = 0.0;
  Tpt[3 * 4 + 1] = 0.0;
  Tpt[3 * 4 + 2] = 0.0;
  Tpt[3 * 4 + 3] = 1.0;
  return 1;
}

/**
 * @brief 把DH参数转换为齐次变换矩阵(待验证)
 * @note  成功输出1，失败输出0;
 * @param thetadh : DH的theta参数
 * @param alfadh ：DH的alfa参数
 * @param rdh ：DH的r参数
 * @param ddh ：DH的d参数
 * @param Tdh ：DH的齐次变换矩阵
 * @param numT : 齐次变换矩阵的元素个数
 */
int DH1line(float thetadh, float alfadh, float rdh, float ddh, float *Tdh, int numT )
{
  //判断矩阵元素是否合法
  if (numT != 16) return 0;
  // creats Denavit-Hartenberg homogeneous transformation matrix
  // first row
  Tdh[0 * 4 + 0] = cos(thetadh);
  Tdh[0 * 4 + 1] = -sin(thetadh) * cos(alfadh);
  Tdh[0 * 4 + 2] = sin(thetadh) * sin(alfadh);
  Tdh[0 * 4 + 3] = rdh * cos(thetadh);
  // second row
  Tdh[1 * 4 + 0] = sin(thetadh);
  Tdh[1 * 4 + 1] = cos(thetadh) * cos(alfadh);
  Tdh[1 * 4 + 2] = -cos(thetadh) * sin(alfadh);
  Tdh[1 * 4 + 3] = rdh * sin(thetadh);
  // third row
  Tdh[2 * 4 + 0] = 0.0;
  Tdh[2 * 4 + 1] = sin(alfadh);
  Tdh[2 * 4 + 2] = cos(alfadh);
  Tdh[2 * 4 + 3] = ddh;
  // forth row
  Tdh[3 * 4 + 0] = 0.0;
  Tdh[3 * 4 + 1] = 0.0;
  Tdh[3 * 4 + 2] = 0.0;
  Tdh[3 * 4 + 3] = 1.0;
  return 1;
}

/**
 * @brief 输入一个m*n的矩阵A，并将该矩阵复制给B;
 * @note  矩阵的复制，成功输出1，失败输出0;
 * @param A : m*n的输入矩阵
 * @param B ：m*n的输出矩阵
 * @param m ：矩阵的行数
 * @param n ：矩阵的列数
 * @param numA : 矩阵A的元素个数
 * @param numB : 矩阵B的元素个数
 */
int MatrixCopy(float *A, int n, int m, float *B,int numA, int numB)
{
  //判断元素是否合法
  if((numA != m*n)||(numB != m*n)) return 0;
  int i, j;
  for (i = 0; i < m; i++)
    for (j = 0; j < n; j++)
    {
      B[n * i + j] = A[n * i + j];
    }
  return 1;
}

/**
 * @brief 输入一个m*p的矩阵A，输入一个p*n的矩阵B，并将两个矩阵相乘得到C
 * @note  矩阵的乘法，成功输出1，失败输出0;
 * @param A : m*p的输入矩阵
 * @param B ：p*n的输入矩阵
 * @param m ：矩阵A的行数
 * @param p : 矩阵A的列数 = 矩阵B的行数
 * @param n ：矩阵B的列数
 * @param C ：m*n的输出矩阵
 * @param numA : 矩阵A的元素个数
 * @param numB : 矩阵B的元素个数
 * @param numC : 矩阵C的元素个数
 */
int MatrixMultiply(float *A, float *B, int m, int p, int n, float *C,int numA ,int numB,int numC)
{
  //判断元素是否合法
  if((numA != m*p)||(numB != p*n)||(numC != m*n)) return 0;
  int i, j, k;
  for (i = 0; i < m; i++)
    for (j = 0; j < n; j++)
    {
      C[n * i + j] = 0;
      for (k = 0; k < p; k++)
        C[n * i + j] = C[n * i + j] + A[p * i + k] * B[n * k + j];
    }
  return 1;
}

/**
 * @brief 输入两个m*n的矩阵A，B，并将两个矩阵相加得到C
 * @note  矩阵的加法，成功输出1，失败输出0
 * @param A : m*n的输入矩阵
 * @param B ：m*n的输入矩阵
 * @param m ：矩阵的行数
 * @param n ：矩阵的列数
 * @param C ：m*n的输出矩阵
 * @param numA : 矩阵A的元素个数
 * @param numB : 矩阵B的元素个数
 * @param numC : 矩阵C的元素个数
 */
int MatrixAdd(float *A, float *B, int m, int n, float *C,int numA ,int numB,int numC)
{
  //判断元素是否合法
  if((numA != m*n)||(numB != m*n)||(numC != m*n)) return 0;
  int i, j;
  for (i = 0; i < m; i++)
    for (j = 0; j < n; j++)
      C[n * i + j] = A[n * i + j] + B[n * i + j];
  return 1;
}

/**
 * @brief 输入两个m*n的矩阵A，B，并将两个矩阵相减得到C
 * @note  矩阵的减法，成功输出1，失败输出0
 * @param A : m*n的输入矩阵
 * @param B ：m*n的输入矩阵
 * @param m ：矩阵的行数
 * @param n ：矩阵的列数
 * @param C ：m*n的输出矩阵
 * @param numA : 矩阵A的元素个数
 * @param numB : 矩阵B的元素个数
 * @param numC : 矩阵C的元素个数
 */
int MatrixSubtract(float *A, float *B, int m, int n, float *C,int numA ,int numB,int numC)
{
  //判断元素个数是否合法
  if((numA != m*n)||(numB != m*n)||(numC != m*n)) return 0;
  int i, j;
  for (i = 0; i < m; i++)
    for (j = 0; j < n; j++)
      {C[n * i + j] = A[n * i + j] - B[n * i + j];
      }
  return 1;
}

/**
 * @brief 输入一个m*n的矩阵A，将该矩阵进行转置并输出给矩阵C
 * @note  矩阵的转置,成功输出1，失败输出0
 * @param A : m*n的输入矩阵
 * @param m ：矩阵的行数
 * @param n ：矩阵的列数
 * @param C ：n*m的输出矩阵
 * @param numA : 矩阵A的元素个数
 * @param numB : 矩阵B的元素个数
 */
int MatrixTranspose(float *A, int m, int n, float *C,int numA ,int numB)
{
  //判断元素个数是否合法
  if((numA != m*n)||(numB != m*n)) return 0;
  int i, j;
  for (i = 0; i < m; i++)
    for (j = 0; j < n; j++)
      C[m * j + i] = A[n * i + j];
  return 1;
}

/**
 * @brief 输入一个m*n的矩阵和一个系数k，将矩阵进行数乘
 * @note  将A矩阵扩大K倍，1表示成功，0表示失败
 * @param A : 一个m*n的矩阵
 * @param m ：矩阵的行数
 * @param n ：矩阵的列数
 * @param k ：系数K
 * @param num : 矩阵大小
 */
int MatrixScale(float *A, int m, int n, float k, int num)
{
  //判断元数个数是否合法
  if(num != m*n) return 0;
  for (int i = 0; i < m; i++)
    for (int j = 0; j < n; j++)
      A[n * i + j] = A[n * i + j] * k;
  return 1;
}
